#ifndef ROBORT_GOAL_FACTORY_H
#define ROBORT_GOAL_FACTORY_H
#include<Eigen/Core>
#include<ros/ros.h>
#include<tf/tf.h>
#include<tf/transform_listener.h>
#include<geometry_msgs/PoseStamped.h>

#include<actionlib/client/simple_action_client.h>
#include "../executor/chassis_executor.h"

#include "example_behavior/line_iterator.h"

#include "../blackboard/blackboard.h"

#include "../behavior_tree/behavior_node.h"
#include "../behavior_tree/behavior_state.h"

#include "io/io.h"
#include "../proto/decision.pb.h"

//#include "roborts_msgs/SwingDefend.h"

namespace roborts_decision{
  class GoalFactory{
  public:
    typedef roborts_msgs::GlobalPlannerGoal GlobalGoal;
    typedef roborts_msgs::LocalPlannerGoal LocalGoal;
    typedef std::shared_ptr<GoalFactory> GoalFactoryPtr;
    

  protected : 
    Blackboard::Ptr blackboard_ptr_;
    std::shared_ptr<ChassisExecutor> chassis_executor_ptr_;
    std::shared_ptr<GimbalExecutor> gimbal_executor_ptr_;
    
    
  public:
    GoalFactory(const Blackboard::Ptr &blackboard_ptr):
        blackboard_ptr_(blackboard_ptr){
        chassis_executor_ptr_ = blackboard_ptr_->GetChassisExecutor();
        gimbal_executor_ptr_ = blackboard_ptr_ ->GetGimbalExecutor();
        //ros::NodeHandle sd;
        //ros::ServiceServer service = sd.advertiseService("swing_defend_", GoalFactory::SwingDefend,this);
    
        }

    ~GoalFactory() = default;

    void BuffGoal(int kind_of_buff) {
      geometry_msgs::PoseStamped buff_goal;

      buff_goal.header.frame_id = "map";
      buff_goal.pose.position.x =0;
      buff_goal.pose.position.y =0;
      buff_goal.pose.position.z =0;          //此处对x，y，z赋值仅是初始化
      //try{
        tf::Quaternion quaternion = tf::createQuaternionFromRPY(0,0,0);   //初始化
      //}
      //catch(tf::TransformException& ex){
        //ROS_ERROR("transform error: %s",ex.what());
      //}
      buff_goal.pose.orientation.x = quaternion.x();
      buff_goal.pose.orientation.y = quaternion.y();
      buff_goal.pose.orientation.z = quaternion.z();
      buff_goal.pose.orientation.w = quaternion.w();
      blackboard_ptr_->BuffPose(kind_of_buff);    
      buff_goal = blackboard_ptr_->GetBuffPose();
      chassis_executor_ptr_->Execute(buff_goal);
    }

    //受到攻击后转向合适的方向
    void TurnToDetectedDirection() {      
      if(blackboard_ptr_->IfTurn()){
        double q=1;   
        double d_yaw=0;
        switch (blackboard_ptr_->ArmorDamageSource()) {
          case 0:  //前方
              break;  
          case 1:  //左方
              d_yaw = M_PI / 2.;
              break;
          case 2:  //后方
              d_yaw = M_PI;
              q=0.5;
              break;
          case 3:  //右方
              d_yaw = -M_PI / 2.;
              break;
          default:
              return;
        } 
        try{ 
          ros::Rate rate(q);
          geometry_msgs::PoseStamped hurt_pose;
          auto quaternion = tf::createQuaternionMsgFromYaw(d_yaw);
          hurt_pose.header.frame_id = "base_link";
          hurt_pose.header.stamp = ros::Time::now();
          hurt_pose.pose.orientation = quaternion;
          chassis_executor_ptr_->Execute(hurt_pose);
          rate.sleep(); 
        }
        catch(std::exception& e){
          ROS_ERROR("ERROR: %S",e.what());
        }
      } 
    }

    void LeaveFormBuff(){
      geometry_msgs::PoseStamped leave_buff_goal;
      try{
        leave_buff_goal.header.frame_id = "map";
        leave_buff_goal.pose.position.x =0;
        leave_buff_goal.pose.position.y =0;
        leave_buff_goal.pose.position.z =0;          
        tf::Quaternion quaternion = tf::createQuaternionFromRPY(0,0,0);  
        leave_buff_goal.pose.orientation.x = quaternion.x();
        leave_buff_goal.pose.orientation.y = quaternion.y();
        leave_buff_goal.pose.orientation.z = quaternion.z();
        leave_buff_goal.pose.orientation.w = quaternion.w(); 
      
        if(blackboard_ptr_->IsNearRefresh()){        //GetGameTime()距下一次刷新时间是否小于1.5秒
          leave_buff_goal = blackboard_ptr_->GetWaitRefreshPos();
          chassis_executor_ptr_->Execute(leave_buff_goal);
        }
      }
      catch(std::exception& e){
        ROS_ERROR("ERROR: %s",e.what());
      }
    }

    /*void ShootGoal(){
      roborts_msgs::ShootCmd shoot_mode_msg;
      //若敌人超出设定的射击范围，则取消射击
      if(blackboard_ptr_->GetEnemyDistance()>=blackboard_ptr_->GetShootingRange()){
        blackboard_ptr_->SetShootMode(0);
        shoot_mode_msg.request.mode = static_cast<uint8_t >(blackboard_ptr_->GetShootMode());
        if(shootcmd_client_.call(shoot_mode_msg) && shoot_mode_msg.response.received){
          ROS_INFO("Stop Shooting !");
          return;
        }else{
          ROS_ERROR("stop shooting failed !");
          return;
        }        
      
      const ShootMode &shoot_mode;
      shoot_mode = blackboard_ptr_->GetShootMode();
      shoot_mode_msg.request.mode = static_cast<uint8_t >(shoot_mode);
      if(shootcmd_client_.call(shoot_mode_msg) && shoot_mode_msg.response.received){
          std::cout<<"shoot mode is"<< static_cast<int>(shoot_mode)<<std::endl;
        }else{
          ROS_ERROR("set shoot mode failed!");
        }        
      }
      roborts_msgs::ShootCmd shoot_number_msg;
      // const unsigned int &shoot_number;
      const unsigned int shoot _number = GetShootNum();
      shoot_number_msg.request.number = static_cast<int8_t>(shoot_number);
      if(shootcmd_client_.call(shoot_number_msg) && shoot_number_msg.response.received){
          std::cout<<"shoot number is"<< static_cast<int>(shoot_number)<<std::endl;
        }else{
          ROS_ERROR("set shoot number failed!");
        }        
    }*/

/*  //   可能会导致重复发射，达不到节约的目的
    void ShootGoal(const ShootMode &shoot_mode,const unsigned int &shoot_number){
      if(shoot_mode!=blackboard_ptr_->GetShootMode()){                               //设置发射模式（0：停止 1：射击一次 2：连续射击）
        roborts_msgs::ShootCmd shoot_mode_msg;
        shoot_mode_msg.request.mode = static_cast<uint8_t >(shoot_mode);
        if(shootcmd_client_.call(shoot_mode_msg) && shoot_mode_msg.response.received){
          blackboard_ptr_->SetShootNum(shoot_mode);
          std::cout<<"set shoot mode to"<< static_cast<int>(shoot_mode)<<std::endl;
        }else{
          ROS_ERROR("set shoot mode failed!");
        }        
      }
      if(shoot_number!=blackboard_ptr_->GetShootNum()){                           //设置发射子弹的数量
        roborts_msgs::ShootCmd shoot_number_msg;
        shoot_number_msg.request.number = static_cast<int8_t>(shoot_number);
        if(shootcmd_client_.call(shoot_number_msg) && shoot_number_msg.response.received){
          blackboard_ptr_->SetShootNum(shoot_number);
          std::cout<<"set shoot number to"<< static_cast<int>(shoot_number)<<std::endl;
        }else{
          ROS_ERROR("set shoot number failed!");
        }        
      }
    }
*/
    /*void ChaseGoal() {
      auto robot_map_pose = blackboard_ptr_->GetRobotMapPose();             
      std::vector<geometry_msgs::PoseStamped> chase_buffer=blackboard_ptr_->GetChaseBuffer();
      unsigned int chase_count=blackboard_ptr_->GetChaseCount();   
            
      if (blackboard_ptr_->IsEnemyLost()&&blackboard_ptr_->GetChaseCount() >= 1) {   //
          blackboard_ptr_->SetEnemyLost(false);       
          blackboard_ptr_->SetSearchCount(5);      
      }

      chase_buffer[chase_count++ % 2] = blackboard_ptr_->GetEnemyPose();                                  //
      chase_count = chase_count % 2;       

      auto dx = chase_buffer[(chase_count + 2 - 1) % 2].pose.position.x - robot_map_pose.pose.position.x;        //为什么+2-1
      auto dy = chase_buffer[(chase_count + 2 - 1) % 2].pose.position.y - robot_map_pose.pose.position.y;        //为什么+2-1
      auto yaw = std::atan2(dy, dx);                                   

      blackboard_ptr_->SetChaseBuffer(chase_buffer);
      blackboard_ptr_->SetChaseCount(chase_count);                          
    
      // 若敌我距离为1-2米，则视为追到，此时取消追击 
      if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) >= 1.0 && std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) <= 2.0) {       
        if (blackboard_ptr_->GetCancelFlag()) {      
          chassis_executor_ptr_->Cancel();
          blackboard_ptr_->SetCancelFlag(false);
        }
        blackboard_ptr_->SetChase(true);
        return;
      } else {                                       
        //若未追到，则将坐标转化为代价地图中的坐标计算
        auto orientation = tf::createQuaternionMsgFromYaw(yaw);
        geometry_msgs::PoseStamped reduce_goal;
        reduce_goal.pose.orientation = robot_map_pose.pose.orientation;

        reduce_goal.header.frame_id = "map";
        reduce_goal.header.stamp = ros::Time::now();
        reduce_goal.pose.position.x = chase_buffer[(chase_count + 2 - 1) % 2].pose.position.x - 1.2 * cos(yaw);
        reduce_goal.pose.position.y = chase_buffer[(chase_count + 2 - 1) % 2].pose.position.y - 1.2 * sin(yaw);
        auto enemy_x = reduce_goal.pose.position.x;
        auto enemy_y = reduce_goal.pose.position.y;
        reduce_goal.pose.position.z = 1;
        unsigned int goal_cell_x, goal_cell_y;

        auto get_enemy_cell = blackboard_ptr_->GetCostMap2D()->World2Map(enemy_x,         //坐标转化
                                                                   enemy_y,
                                                                   goal_cell_x,
                                                                   goal_cell_y);

        if (!get_enemy_cell) {
          ROS_ERROR("The conversion from Word to Map was failed!");
          return;
        }

        auto robot_x = robot_map_pose.pose.position.x;
        auto robot_y = robot_map_pose.pose.position.y;
        unsigned int robot_cell_x, robot_cell_y;
        double goal_x, goal_y;
        auto get_robot_cell = blackboard_ptr_->GetCostMap2D()->World2Map(robot_x,          //坐标转化
                                              robot_y,
                                              robot_cell_x,
                                              robot_cell_y);

        if (!get_robot_cell) {
          ROS_ERROR("The conversion from Word to Map was failed!");
          return;
        }

        if (blackboard_ptr_->GetCostMap2D()->GetCost(goal_cell_x, goal_cell_y) >= 253) {     //表示敌我之间有障碍
          bool find_goal = false;
          //计算距离敌人最近的没有障碍的路径
          for(FastLineIterator line( goal_cell_x, goal_cell_y, robot_cell_x, robot_cell_x); line.IsValid(); line.Advance()) {     

            auto point_cost = blackboard_ptr_->GetCostMap2D()->GetCost((unsigned int) (line.GetX()), (unsigned int) (line.GetY())); 

            if(point_cost >= 253){
              continue;
            } else {
              find_goal = true;
              blackboard_ptr_->GetCostMap2D()->Map2World((unsigned int) (line.GetX()),
                                                     (unsigned int) (line.GetY()),
                                                     goal_x,
                                                     goal_y);
              reduce_goal.pose.position.x = goal_x;
              reduce_goal.pose.position.y = goal_y;
              break;
            }
          }
          if (find_goal) {
            blackboard_ptr_->SetCancelFlag(true);
            chassis_executor_ptr_->Execute(reduce_goal);
          } else {
          //若未找到目标，则取消追击
            if (blackboard_ptr_->GetCancelFlag()) {                
              chassis_executor_ptr_->Cancel();
              blackboard_ptr_->SetCancelFlag(false);
            }
            return;
          }
        } else {  
          //若敌我之间无障碍，则直接向敌人的坐标前进                                              
          blackboard_ptr_->SetCancelFlag(true);
          chassis_executor_ptr_->Execute(reduce_goal);
        }
      }
    }*/
    //FIXME 已修改
    void ChaseGoal() {
      int chase_num = blackboard_ptr_->GetChaseEnemyNum();
      int near_num = blackboard_ptr_->GetNearEnemyNUm();
      //若正在追击的目标相隔远，且另一个敌方机器人相隔较近，则改变追击目标
      if(blackboard_ptr_->GetEnemyDistance()<=3.0 && (chase_num != near_num)){
        chase_num = near_num;
        blackboard_ptr_->SetChaseEnemyNUm(near_num);
      }

      auto robot_map_pose = blackboard_ptr_->GetRobotMapPose();
      std::vector<geometry_msgs::PoseStamped> chase_buffer=blackboard_ptr_->GetChaseGoal();  
      auto dx = chase_buffer[chase_num].pose.position.x - robot_map_pose.pose.position.x;        
      auto dy = chase_buffer[chase_num].pose.position.y - robot_map_pose.pose.position.y;        
      auto yaw = std::atan2(dy, dx);                                   
                         
      // 若敌我距离为1-2米，则视为追到，此时取消追击 
      if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) >= 1.0 && std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) <= 2.0) {       
        if (blackboard_ptr_->GetCancelFlag()) {      
          chassis_executor_ptr_->Cancel();
          blackboard_ptr_->SetCancelFlag(false);
        }
        blackboard_ptr_->SetChase(true);
        return;
      } else {                                       
        //若未追到，则将坐标转化为代价地图中的坐标计算
        auto orientation = tf::createQuaternionMsgFromYaw(yaw);
        geometry_msgs::PoseStamped reduce_goal;
        reduce_goal.pose.orientation = robot_map_pose.pose.orientation;
        reduce_goal.header.frame_id = "map";
        reduce_goal.header.stamp = ros::Time::now();
        reduce_goal.pose.position.x = chase_buffer[chase_num].pose.position.x - 1.2 * cos(yaw);
        reduce_goal.pose.position.y = chase_buffer[chase_num].pose.position.y - 1.2 * sin(yaw);
        auto enemy_x = reduce_goal.pose.position.x;
        auto enemy_y = reduce_goal.pose.position.y;
        reduce_goal.pose.position.z = 1;
        unsigned int goal_cell_x, goal_cell_y;

        auto get_enemy_cell = blackboard_ptr_->GetCostMap2D()->World2Map(enemy_x,         //坐标转化
                                                                   enemy_y,
                                                                   goal_cell_x,
                                                                   goal_cell_y);

        if (!get_enemy_cell) {
          ROS_ERROR("The conversion from Word to Map was failed!");
          return;
        }

        auto robot_x = robot_map_pose.pose.position.x;
        auto robot_y = robot_map_pose.pose.position.y;
        unsigned int robot_cell_x, robot_cell_y;
        double goal_x, goal_y;
        auto get_robot_cell = blackboard_ptr_->GetCostMap2D()->World2Map(robot_x,          //坐标转化
                                              robot_y,
                                              robot_cell_x,
                                              robot_cell_y);

        if (!get_robot_cell) {
          ROS_ERROR("The conversion from Word to Map was failed!");
          return;
        }

        if (blackboard_ptr_->GetCostMap2D()->GetCost(goal_cell_x, goal_cell_y) >= 253) {     //表示敌我之间有障碍
          bool find_goal = false;
          //计算距离敌人最近的没有障碍的路径
          for(FastLineIterator line( goal_cell_x, goal_cell_y, robot_cell_x, robot_cell_x); line.IsValid(); line.Advance()) {     

            auto point_cost = blackboard_ptr_->GetCostMap2D()->GetCost((unsigned int) (line.GetX()), (unsigned int) (line.GetY())); 

            if(point_cost >= 253){
              continue;
            } else {
              find_goal = true;
              blackboard_ptr_->GetCostMap2D()->Map2World((unsigned int) (line.GetX()),
                                                     (unsigned int) (line.GetY()),
                                                     goal_x,
                                                     goal_y);
              reduce_goal.pose.position.x = goal_x;
              reduce_goal.pose.position.y = goal_y;
              break;
            }
          }
          if (find_goal) {
            blackboard_ptr_->SetCancelFlag(true);
            chassis_executor_ptr_->Execute(reduce_goal);
          } else {
          //若未找到目标，则取消追击
            if (blackboard_ptr_->GetCancelFlag()) {                
              chassis_executor_ptr_->Cancel();
              blackboard_ptr_->SetCancelFlag(false);
            }
            return;
          }
        } else {  
          //若敌我之间无障碍，则直接向敌人的坐标前进                                              
          blackboard_ptr_->SetCancelFlag(true);
          chassis_executor_ptr_->Execute(reduce_goal);
        }
      }
    }



    void FollowGoal(){
      auto robot_map_pose = blackboard_ptr_->GetRobotMapPose();
      auto friend_pose =blackboard_ptr_->GetTeammatePose();
                                 
      auto dx = friend_pose.pose.position.x - robot_map_pose.pose.position.x;        
      auto dy = friend_pose.pose.position.y - robot_map_pose.pose.position.y;        
      auto yaw = std::atan2(dy, dx); 

      //跟随距离设置为1-2米，在此范围内则停止
      if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) >= 1.0 && std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) <= 2.0) {       
        return;
      } else {                                       
        // 不在跟随范围内则跟随
        auto orientation = tf::createQuaternionMsgFromYaw(yaw);
        geometry_msgs::PoseStamped reduce_goal;
        reduce_goal.pose.orientation = robot_map_pose.pose.orientation;

        reduce_goal.header.frame_id = "map";
        reduce_goal.header.stamp = ros::Time::now();
        reduce_goal.pose.position.x = friend_pose.pose.position.x - 1.2 * cos(yaw);   //
        reduce_goal.pose.position.y = friend_pose.pose.position.y - 1.2 * sin(yaw);
        auto firend_x = reduce_goal.pose.position.x;
        auto friend_y = reduce_goal.pose.position.y;
        reduce_goal.pose.position.z = 1;
        unsigned int goal_cell_x, goal_cell_y;

        auto get_enemy_cell = blackboard_ptr_->GetCostMap2D()->World2Map(firend_x,         
                                                                   friend_y,
                                                                   goal_cell_x,
                                                                   goal_cell_y);
        if (!get_enemy_cell) {
          ROS_ERROR("The conversion from Word to Map was failed!");
          return;
        }

        auto robot_x = robot_map_pose.pose.position.x;
        auto robot_y = robot_map_pose.pose.position.y;
        unsigned int robot_cell_x, robot_cell_y;
        double goal_x, goal_y;
        auto get_robot_cell = blackboard_ptr_->GetCostMap2D()->World2Map(robot_x,          //坐标转化
                                              robot_y,
                                              robot_cell_x,
                                              robot_cell_y);
        if (!get_robot_cell) {
          ROS_ERROR("The conversion from Word to Map was failed!");
          return;
        }

        if (blackboard_ptr_->GetCostMap2D()->GetCost(goal_cell_x, goal_cell_y) >= 253) {
          bool find_goal = false;
          // 计算1、2号车之间最近的没有障碍的路径
          for(FastLineIterator line( goal_cell_x, goal_cell_y, robot_cell_x, robot_cell_x); line.IsValid(); line.Advance()) {     
            auto point_cost = blackboard_ptr_->GetCostMap2D()->GetCost((unsigned int) (line.GetX()), (unsigned int) (line.GetY())); 
            if(point_cost >= 253){
              continue;
            } else {
              find_goal = true;
              blackboard_ptr_->GetCostMap2D()->Map2World((unsigned int) (line.GetX()),
                                                     (unsigned int) (line.GetY()),
                                                     goal_x,
                                                     goal_y);
              reduce_goal.pose.position.x = goal_x;
              reduce_goal.pose.position.y = goal_y;
              break;
            }
          }

          if (find_goal) {
            chassis_executor_ptr_->Execute(reduce_goal);
          } else {
            return;
          }
        } else {  
          //若1、2号车之间无障碍，则直接跟随                                              
          chassis_executor_ptr_->Execute(reduce_goal);
        }
      }      
    }
  

    void BackToBootArea(){
      geometry_msgs::PoseStamped boot_pose ; 
      boot_pose.header.frame_id = "map";
      boot_pose.pose.orientation.x = 0;
      boot_pose.pose.orientation.y = 0;
      boot_pose.pose.orientation.z = 0;
      boot_pose.pose.orientation.w = 1;

      boot_pose.pose.position.x = 0;
      boot_pose.pose.position.y = 0;
      boot_pose.pose.position.z = 0;
      
      boot_pose = blackboard_ptr_->LoadBootPosition();
      auto robot_map_pose = blackboard_ptr_->GetRobotMapPose();
      auto dx = boot_pose.pose.position.x - robot_map_pose.pose.position.x;
      auto dy = boot_pose.pose.position.y - robot_map_pose.pose.position.y;

      auto boot_yaw = tf::getYaw(boot_pose.pose.orientation);
      auto robot_yaw = tf::getYaw(robot_map_pose.pose.orientation);

      tf::Quaternion rot1, rot2;
      tf::quaternionMsgToTF(boot_pose.pose.orientation, rot1);
      tf::quaternionMsgToTF(robot_map_pose.pose.orientation, rot2);
      auto d_yaw =  rot1.angleShortestPath(rot2);

      if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) > 0.2 || d_yaw > 0.5) {
        chassis_executor_ptr_->Execute(boot_pose);
      }
    }

  

    //bool SwingDefend(roborts_msgs::SwingDefend::Request &req, roborts_msgs::SwingDefend::Response& res)
    void SwingDefend()
    {
      float move_dst = 0.20; // 在摆动防御是车身左右移动的距离, 暂定20cm
      geometry_msgs::PoseStamped swing_goal;
      swing_goal.header.frame_id = "map";
      swing_goal = blackboard_ptr_->GetRobotMapPose();
      tf::Quaternion cur_q;
      tf::quaternionMsgToTF(swing_goal.pose.orientation, cur_q);
      double r, p, y;
      tf::Matrix3x3(cur_q).getRPY(r, p, y); // 从位置信息中获取车身的角度
      float move_x = asin(y) * move_dst;    // x轴方向上移动距离
      float move_y = -acos(y) * move_dst;   // y轴方向上移动距离
      swing_goal.pose.position.z = 0;
      //ros::Rate rate(req.rate);
      ros::Rate rate(20);
      //res.is_swing = true;
      while (ros::ok() && blackboard_ptr_->IsSwing())
      {
        // 更新 x, y 轴坐标
        swing_goal.pose.position.x += move_x;
        swing_goal.pose.position.y += move_y;
        chassis_executor_ptr_->Execute(swing_goal);
        rate.sleep();
        swing_goal.pose.position.x -= move_x;
        swing_goal.pose.position.y -= move_y;
        chassis_executor_ptr_->Execute(swing_goal);
        rate.sleep();
      }
      //return true;
    } //SwingDefend

      // 开启摩擦轮,准备射击
    /*void SetFricWhrl()
    {
        blackboard_ptr_->SetFricWhl();
    }

    // 关闭摩擦轮
    void OffFricWhl()
    {
        blackboard_ptr_->OffFricWhl();
    }*/

    // 转动云台以扫描视野范围内是否有敌人
    void ScanView()
    {
      double angle_min = 0;
      double angle_max = 3.14;                // 这两个参数待定,云台的可运动范围
      double angle = 0;                       // 云台转动时单次运动角度
      short dir = 1;                          // 转动方向
      roborts_msgs::GimbalAngle gimbal_angle_msg;
      gimbal_angle_msg.pitch_mode = false;
      gimbal_angle_msg.pitch_angle = 0;
      gimbal_angle_msg.yaw_mode = true;
      gimbal_angle_msg.yaw_angle = 1.57;       // 云台转动的角度范围暂时未知,待实测修改    ---waiting to be modified
      ros::Rate rate(15);
      try
      {
        while (ros::ok() && blackboard_ptr_->IsGimbalView())
        {
          if (angle_min >= gimbal_angle_msg.yaw_angle || gimbal_angle_msg.yaw_angle >= angle_max)
              dir *= -1; //若转动的角度到达极限, 则调转方向
          gimbal_angle_msg.yaw_angle += dir * angle;
          gimbal_executor_ptr_->Execute(gimbal_angle_msg);
          rate.sleep();
        }
      }
      catch (...)
      {
        ROS_INFO("gimbal scan error . ");
        blackboard_ptr_->SetGimbalScan(false);
        return;
      }
      blackboard_ptr_->SetGimbalScan(false);
      ROS_INFO("gimbal-scan exit. ");
      return;
    }

    // 当与哨岗的联系出现问题,并且视野中无法获取到敌人时,执行搜索动作,直到遭遇敌人
    // 搜索路径预先设定好, 在 prototxt 文件中读取
    void SearchGoal(){
      geometry_msgs::PoseStamped robort_pose = blackboard_ptr_->GetRobotMapPose();
      std::vector<geometry_msgs::PoseStamped> path = blackboard_ptr_->GetSearchRegion1();
      if(robort_pose.pose.position.x > 4.040)
        path = blackboard_ptr_->GetSearchRegion2();
      if(path.empty()){
        ROS_WARN("search path empty error !");
        return;
      }
      geometry_msgs::PoseStamped s_pose;
      int count = blackboard_ptr_->GetSearchCount();
      int size =  blackboard_ptr_->GetSearchPointSize();
      try{
        chassis_executor_ptr_->Execute(path[count % size]);
      }
      catch(std::exception& e){
        ROS_WARN("search execute error %s: ", e.what());
      }
      blackboard_ptr_->SetSearchCount(++count);
    }

    void Escape(){
      std::vector<geometry_msgs::PoseStamped> escape_points = blackboard_ptr_ -> GetEscapePoints();
      geometry_msgs::PoseStamped robot_pose = blackboard_ptr_->GetRobotMapPose();
      int region = blackboard_ptr_->GetCurrentReg(robot_pose);
      geometry_msgs::PoseStamped escape_goal = escape_points[(region+2)%4];
      if(blackboard_ptr_->GetEnemyDistance()<=2.0){
          auto quaternion = tf::createQuaternionMsgFromYaw(blackboard_ptr_->GetEnemyYaw());
          escape_goal.pose.orientation = quaternion;
      }
      try{
        if(!blackboard_ptr_->IsEscape()){
          chassis_executor_ptr_->Execute(escape_goal);
          blackboard_ptr_->SetEscape(true);
        }
        else
          return;
        ROS_INFO("escapping ...");
      }
      catch(std::exception& e){
        ROS_WARN("escape error occured : %s", e.what());
      }
    }// escape

    void CancelGoal() {
      ROS_INFO("Cancel Goal!");
      blackboard_ptr_->SetCancelFlag(false);
      // TODO 这两行实际是需要的，在和路径规划整合代码的时候取消规划的内容(不一定是下面这两句)
      //chassis_executor_ptr_->cancelGlobalGoal();
      //chassis_executor_ptr_->cancelLocalGoal();
      blackboard_ptr_->SetActionState(BehaviorState::IDLE);
    }

    // 放弃云台控制权, 取消转动云台进行扫描的行为
    void CancelScan(){
        if(blackboard_ptr_->IsGimbalView()){
            blackboard_ptr_->SetGimbalView(false);
            ROS_INFO("cancel gimbal-scan action .");
        }
    }//cancel scan

    void CancelChassis(){
      // FIXME 这块的作用是禁止地盘的运动
      chassis_executor_ptr_->Cancel();
    }

    void CancelGimbal(){
      // FIXME 这块作用是让云台不动，所以也要让视觉不能动云台
      gimbal_executor_ptr_ -> Cancel();
    }
    // 这两个取消行为待定
    // FIXME 禁止射击可能需要再设计一个函数，向视觉发送信号不要射击，或者直接视觉自己订阅是否禁止射击
    //自瞄自己订阅
  };  // goalfactory
} //roborts_decision

#endif  // ROBORT_GOAL_FACTORY_H
